//
//  LAGTrackBasicLogics.c
//  LAMapBasicDemo
//
//  Created by 罗亮富 on 2017/12/12.
//  Copyright © 2017年 2Bulu. All rights reserved.
//

#include "LAGTrackFiltLogics.h"
#include "gps.h"

void printPointsForSeg(LocationPointsSegmentType seg)
{
    for(int i = 0; i < seg.pointsCount; i++)
    {
        LocationPointType pt = *(seg.dataBuffer + i);
        printf("(%.7f,%.7f)-",pt.longitude,pt.latitude);
    }
    
    printf("\n");
}

//返回被删除坐标点数 处理完成后pointsWindow数组中的点将被删除，pointCount传入的坐标点数
unsigned int deleteShiftedPoints1(LocationPointType *pointsWindow, unsigned int pointCount, double ninmumShift)
{
    
    if(pointCount < 4)
        return 0;
    
    LocationPointType newPoint = *(pointsWindow+pointCount-1);
    LocationPointType prePoint = *(pointsWindow+pointCount-2);//前一个点
    LocationPointType RefPoint;//参考点,初始值为前一个点
    LocationPointType preRefPoint; //参考点前一个点
 //   LocationPointType prepreRefPoint; //参考点前两个点
    LocationPointType refPointPre; //参考点后一个点
    double newDis = calculateDistanceBetweenPoints(newPoint, prePoint, false);//当前点和前一个点距离
    double refDis = 0;//当前点和参考点距离
    double preRefDis = 0;//参考点和前一个点的距离
   // double prepreRefDis = 0; //参考点前一个点 和 参考点前两个点 之间的距离
    double refDisPre = 0; //参考点和参考点后一个点 之间的距离

    int shiftStartIndex = -1;//-1表示无效
    
    
    if(newDis > ninmumShift)
    {
        for(int i = pointCount-3; i > 0; i--)
        {
            RefPoint = *(pointsWindow+i);
            refDis = calculateDistanceBetweenPoints(newPoint, RefPoint, false);
            
            refPointPre = *(pointsWindow+i+1);
            refDisPre = calculateDistanceBetweenPoints(RefPoint, refPointPre, false);
            
            if(refDis < newDis || refDis < refDisPre )//考虑拐弯，立交桥似得绕圈
            {
                preRefPoint = *(pointsWindow+i-1);
                
                preRefDis = calculateDistanceBetweenPoints(RefPoint, preRefPoint, false);
                
              //  if((refDisPre > newDis/3 || refDisPre > MININUM_SHIFT_DISTANCE) && (refDisPre > 2.5*preRefDis || newDis > 2.5*preRefDis ))
                if((refDisPre > ninmumShift) && (refDisPre > 2.5*preRefDis || newDis > 2.5*preRefDis ))
                {
                    shiftStartIndex = i+1;
                }

            }

        }
    }
    
    unsigned int count = 0;
    //漂移开始点序号
    if(shiftStartIndex >= 0)
    {
        count =  pointCount-1 - shiftStartIndex;
        *(pointsWindow+shiftStartIndex) = newPoint;//将最后一个点前移到第一个漂移点的位置
    }
    
    return count; //返回被处理点数
}


unsigned int deleteShiftInPointsSegment(LocationPointsSegmentType seg, unsigned int windowLen, double minumShift)
{
    unsigned int wLen = MAX(4,windowLen);
    if(seg.pointsCount < MIN(4,windowLen))
        return 0;

    unsigned int deletedCount = 0;
    unsigned int pointRemain = seg.pointsCount; //剩余保留坐标点数量
    unsigned int N = 0;
    
    if(wLen > 4) //最小窗口从长度4开始逐步递增到wLen(注意不移动窗口)
    {
        LocationPointType *p = seg.dataBuffer;
        for(int j = 4; j<wLen && j<seg.pointsCount; )
        {
            unsigned int c = deleteShiftedPoints1(p,j,minumShift);
            if(c > 0) //说明有被删除的点
            {
                unsigned int l = j - c;//传入的坐标点剩余数量
                memmove(p+l, p+j, (pointRemain-j)*sizeof(LocationPointType));
                
                j -= c;
                deletedCount += c;
                pointRemain = seg.pointsCount - deletedCount;

                if(pointRemain < 4)
                    break;
            }
            
            j++;
            
        }

    }
    
    for(int i = 0; i < pointRemain - 3;)//移动窗口
    {
        LocationPointType *p = seg.dataBuffer + i;
        N = MIN(wLen,pointRemain - i);//自后剩下的长度小于窗口长度，窗口移动同时缩小窗口长度

        unsigned int c = deleteShiftedPoints1(p,N,minumShift);
        if(c > 0) //说明有被删除的点
        {
            unsigned int l = N - c;//传入的坐标点剩余数量
            memmove(p+l, p+N, (pointRemain-i-N)*sizeof(LocationPointType));
            
            deletedCount += c;
            pointRemain = seg.pointsCount - deletedCount;
            i -= c;
            
            if(pointRemain < N)
                break;
        }

        i++;
        if(i < 0)
            i = 0;
    }
    
    seg.pointsCount = pointRemain;
    
    return deletedCount;
}

void kalmanFilterForSegment(LocationPointsSegmentType seg, double noise)
{
    KalmanFilter klFilter = alloc_filter_velocity2d(noise);
    LocationPointType lastPoint = *seg.dataBuffer;
    
    LocationPointType *tmpData = seg.dataBuffer;//malloc(seg.pointsCount*sizeof(LocationPointType));
    
    tmpData[0] = lastPoint;
    
   // double latitude, longitude;
    for ( int i = 1; i < seg.pointsCount; i++)
    {
        LocationPointType point = seg.dataBuffer[i];
        
        update_velocity2d(klFilter, point.latitude, point.longitude, point.timeInterval - lastPoint.timeInterval);
        get_lat_long(klFilter, &tmpData[i].latitude, &tmpData[i].longitude);

        lastPoint = point;
    }
    
   // memcpy(seg.dataBuffer, tmpData, seg.pointsCount*sizeof(LocationPointType));
    
    free_filter(klFilter);
  //  free(tmpData);
}

//根据定位精度自动处理
void kalmanDynamicFilterForSegment(LocationPointsSegmentType seg)
{
    LocationPointType *tmpData = seg.dataBuffer;
    LocationPointType lastPoint;
    LocationPointType point;
    double noise = 150.0;
    int firstIndex = 0;
    int lastAdjustedNoise = 0;//上一个坐标点调整噪声参数
    int cnt = 0;
    int adjustMinAccuracy = 16; //最小纠偏精度，当精度小于该值不纠偏 用int型
    int successiveGoodPointCount = 0;//连续的不需要调整的坐标点数
    
    for ( int i = 1; i < seg.pointsCount; i++)
    {
        point = seg.dataBuffer[i];
        if(point.accuracy < adjustMinAccuracy)//精度小于11m的点不纠正
        {
            successiveGoodPointCount++;
            if(successiveGoodPointCount >= 3)
            {
                lastAdjustedNoise = 0;
                continue;
            }
            else
            {
                noise = point.accuracy;
            }
        }
        else
        {
            successiveGoodPointCount = 0;
            if(point.accuracy >= 100)
                noise = point.accuracy*10;
            else if(point.accuracy >= 60)
                noise = point.accuracy*5;
            else if(point.accuracy > 40)
                noise = point.accuracy*2;
            else
                noise = point.accuracy;
        }
        
        KalmanFilter klFilter = alloc_filter_velocity2d(noise);
        lastAdjustedNoise = noise;
        
        firstIndex = MAX(0, i-10);
        
        lastPoint = seg.dataBuffer[firstIndex];
        for(int k=firstIndex+1; k <= i; k++)
        {
            point = seg.dataBuffer[k];
            update_velocity2d(klFilter, point.latitude, point.longitude, point.timeInterval - lastPoint.timeInterval);
            if(k >= i-successiveGoodPointCount ) //每次只纠偏successiveGoodPointCount+1个点，
            {
                get_lat_long(klFilter, &tmpData[i].latitude, &tmpData[i].longitude);
                
            }
            lastPoint = point;
        }
        free_filter(klFilter);
    }
    
}





